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I . INTRODUCTION 



The Naval Torpedo Station (NAVTORPSTA) Keyport, Washington 
currently operates two three-dimensional (3-D) underwater 
tracking ranges with acoustical capability. The underwater 
tracking range utilizes a sonar transmitter installed in the 
torpedo to be tracked. The transmitter is synchronized with 
a master clock. Timed acoustic pulses are received by bottom 
mounted hydrophone arrays and then relayed via cable to a com- 
puter at the observation site. The computer calculates the 
positional coordinates of the torpedo and plots its trajectory 
through the water. 

The measured data, which consists of the elapsed time 
from transmission of a pulse until its receipt at the hydro- 
phone array, is corrupted with noise due to the combined ef- 
fects of environmental factors and measurement instruments. 

In order to achieve a more accurate plot of the torpedo 
path, editing and smoothing techniques have been applied to 
the data during post run analyses. 

To improve the real-time capability of the 3-D tracking 
ranges, Kalman filtering theory has been applied to the com- 
puter generated X, Y, and Z positional coordinates but is 
not operational [1] . 

In the near future an updated computer system composed 
of three MODCOMPIV computers will be in operation. The 
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majority of the software development requires interfacing 
current tracking programs and other related programs to the 
new computer system. 

An opportunity exists for expanding the capability of 
the tracking system by applying a Kalman filter routine which 
processes measurements of the transit times of the acoustic 
pulses and provides an estimate of the position of the 
tracked object in real time. Prior research in this area 
[2] , revealed that a Kalman filter utilizing a sequential 
estimation approach was desirable. 

The intention is to develop and test a sequential Kalman 
filter tracking algorithm that can be interfaced with the 
current underwater range system. 
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II. DESCRIPTION OF RANGE TRACKING GEOMETRY 



The hydrophone array, consisting of four independent ele- 
ments, defines an orthogonal coordinate system in which transit 
time measurements are made. As shown in Figure 1, four hydro- 
phones X, Y, Z, and C are on four adjacent vertices separated 
by a distance d, along the edge of the cube. The origin of 
the array coordinates is at the center of the cube with the 
orthogonal coordinates parallel to its edge. Positional 
information is computed from the transit times of a periodic 
synchronous acoustic signal travelling from the torpedo to 
the four hydrophones on the array. The range measures the 
tracked torpedo's position every 1.31 seconds to an accuracy 
that is typically within 3 to 30 feet. A more detailed de- 
scription of the range tracking capability is described in 
[3] . 
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Figure 1. 



Geometry of a Tracking Array 
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III. THEORY — EXTENDED KALMAN FILTER 



The equations describing the nonlinear relationship 
between transit times T c , T^, T y , T and the position of 
the tracked torpedo are given by [3] 
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where d = 30 feet and VEL = 4860, which is approximately the 

average velocity of propagation of sound in water at Dabob 
* 

Bay. Since the transit times were readily available and are 
nonlinear functions of position, these equations can be linear- 
ized and Kalman Filter theory applied using the Extended Kalman 
Filter (2] . This procedure producses a real time measurement, 
with filtering on the corrupted transit times T , T , T , and 
T„ without the necessity of converting these times to positions. 

u 



Sound velocity in water varies as a function of temperature, 
salinity, and depth. 
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For the three-dimensional location problem three position 

states (x, y, z) and two velocity states (v , v ) specify 

x y 

target motion. The discrete linear and nonlinear observation 
equations are given by 

x(k + !)=$• x (k) + F • w(k) (3.5) 



and 



z(k) = h ( x (k) , k) + v (k) (3.6) 

In these equations $ and F are constant matrices and h is a 
nonlinear function of the state variable x. w(k) is plant 
excitation noise and v(k) is measurement noise. The plant 
noise and measurement noise are assumed uncorrelated (white) 
with zero mean. 

That is, 

E[w(k) • w T (j) ] = Q' (k)6 kj 

and 

E[v(k) * v T (j)] = R(k)6 kj 

with 

6, . = 1, k = j 
kj 

=0,k/j 

In order to apply the linear filter equation (3.6) is 
expanded in a Taylor series about the best estimate of the 
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the state at that time and only the first-order terms 
kept . 

Equation (3.6) gives 



are 



z(k) = H(k) • x(k) + v(k) 



(3.7) 



where 



x' (k) = x(k/k-l) 

A 

x(k/k-l) is the predicted value of the state before the 
k 1 "* 1 measurement. 

A state error vector is defined by 



H (k) = 



3h 

3x 



(3.7a) 



~ /\ 

X = x (k) - x (k) , 



and a predicted state error vector is defined by 
i(k/k-l) = x (k/k-1) - x (k) . 



The covariance of state error matrix is defined by 
P(k) = E [x (k) • x T (k) ] , 



and the predicted covariance of state error matrix is given 
by 

P (k/k-1) = E[x (k/k-1) • x T (k/k-1)] . 

The state excitation matrix is given by 
Q (k) = T (k) E [w (k) • w T ( k ) ] • f T (k) 
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and the measurement noise covariance matrix is 



R(k) = E [v (k) . v (k) ] 



T 



The Kalman filter equations are given by [4,5]: 



P (k+l/k) = $P (k/k) * + Q ( k ) 

G (k) = P (k/k-1) H T (k) [H(k) • P (k/k-1) IT (k) + R(k) ] 



T 



(3.8a) 



(3.8b) 



P(k) = [I - G/ (k) H (k) ] P (k/k-1) 



(3.8c) 



x (k+l/k = ®x (k/k) 



( 3 . 8d) 



z (k/k— 1 ) = h(x(k/k-l) ,k) ) 



( 3 . 8e) 



x (k) = x (k/k-1) + G(k) [z (k) - z (k/k-1)] 



( 3 . 8f ) 



The Q matrix serves not only to allow for maneuvering but 
also to account for any model inaccuracies, that is, any dis- 
crepancies between the true action of the torpedo and its 
characterization by Equation (3.5) . The Q also serves to 
prevent the gain matrix G(k) from approaching zero by always 
insuring uncertainty in the predicted covariance of error 
matrix P (k+l/k) . 
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IV. PROBLEM DEFINITION — TORPEDO TRACKING WITH THE EXTENDED 

KALMAN FILTER 



A. FILTER EQUATIONS 

Figure 2 illustrates the geometry of the states used for 
the filter. For a constant course, constant-speed torpedo, 
the plant state equations can be described as two second 
order systems, one for X and one for Y. The depth (Z) of the 
torpedo is maintained constant and any velocity in the Z di- 
rection is assumed to be random, uncorrelated with zero mean. 
Z is therefore described as a first order system. 



The plant state equations are 







r- -j 
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x i 0 t v t 
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v x (k) + g 2 {.Yj t ,Y^ t ,k) 
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y (k) +Tv (k) +g (y • ,y. ,k) 

y 3 6 t v t 
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v y < k » + g 4 <^e t ' Y v t , k) 
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z (k) +g 5 (y z ) 



where x(k), and y(k) are the position coordinates at time 
t(k) , v x (k) and v (k) are the X and Y components of velocity. 
T is the time between observations, i.e. t(k+l)-t(k) and z(k) 
is the depth of the torpedo. 
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The excitation terms through g^ are included to account 

for the random changes in speed, heading, and depth which can 

occur in a maneuvering torpedo. The quantities Y v Y, an( j 

11 t 

Y z are the random changes of the torpedo which are assumed 
to be independent, zero mean, rates of change. They have 
variances defined by 



2 
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v t. 


e[ Y 


a e t 2 = 


E [ Y 
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E [y 



2 ] 

] 



The values of the standard deviations are taken from typical 
maneuvering parameters for the torpedo; 

l\ __ c • = 22°/sec 

9 t 

2 

a- =36 ft/sec 

v t 

o^— a. = 1 ft/sec 
z 

The effect of this excitation is to increase the predicted 
value of the covariance of the state error matrix. 

Writing the equations in state form resuts in 



x(k+l) = <4>x(k) + Tw(k) 



(4.2) 



17 



where 



and 



1 T 0 0 0 

0 10 0 0 



$ 



0 



0 1 T 0 



0 0 0 1 0 



0 0 0 0 1 





0 

0 

0 

0 




0 T 



The vector w(k) represents the effect on the states of the 
random excitations. Reference 5 has shown that the vector 
may be calculated from the equations relating torpedo X and 
Y velocity to the torpedo heading and velocity. The X 
velocity is 



x = v = v COS0 

X L L 



which when differentiated gives 



x = ” v t 0 t sin 0 + v t cos 0 t 



(4.4) 
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where 



e t ■ Y e, 



v , = y . 

t 1 v. 



sin 3 t = ^ 



0 -ft. 

= 

1 v t 



and upon substitution of the above relationship in equation 
( 4 . 4 ) , 

v 



x = -v ya + y. — 
y e t v t v t 



( 4 . 5 ) 



Similarly 



y = v = v, sin0. 
■* y t t 



( 4 . 6 ) 



y = v t 0 t cos0 t + v t sin0 t 



and after substitution 



Y VjtY ®t + y< 't 



( 4 . 8 ) 



The depth term is just 



z = y, 



( 4 . 9 ) 
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Thus 



T V v v 

«(k) = [(V* — - Y S v,) Hi V V t y, JL) Y J 
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t v t 1 v y' v '0j. Vx v^v^' 'z' 
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(4.10) 



where from the assumption on the y 's 



V U 



E [w (k) ] = 0 



(4.11) 



The excitation covariance matrix is thus found from 



Q = FE [w (k) w (k) T ] r T 
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(4.13c) 



where the states are evaluated at the current state estimates 
x(k) . Substituting these expressions in the Q matrix results 
in 
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The excitation matrix serves not only to take into ac- 
count the possibility of maneuvering, but of model inaccuracies 
as well [5] . Q is also used to prevent the gains of the filter 
from approaching zero as more and more data is processed , by 
insuring some uncertainty in the predicted state values . 

The observation equations are nonlinear in the states 
and are given by 
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The measurement noises, v(k) 's, are assumed to be zero-mean 
and independent with a covariance matrix 
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pre- 



When the derivatives are taken and evaluated at the 
dieted state values x(k/k-l) = x' (k) the result is 
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where 



DENI = [ (x' (k) +d/2) 2 +(y ' (k) +d/2) 2 +(z ' (k)+d/2) 2 ]^ 



DEN 2 = [ (x' (k) -d/2) 2 + (y ' (k) +d/2) 2 + (z ' (k) +d/2) 2 ] ^ 
DEN 3 = [ (x> (k)+d/2) 2 +(y' (k) -d/2) 2 +(z' (k)+d/2) 2 ]^ 



DEN 4 = [(x* (k)+d/2) 2 +(y ' (k) +d/2) 2 + (z ' (k)-d/2) 2 ]^ . 



The 4> matrix, Q matrix, R matrix, and H matrix are then 
used in the Kalman filter equations (3.8). 

B. THE SEQUENTIAL EXTENDED KALMAN FILTER 

The Kalman filter equations described in the previous 

section represent a system that is characterized by a one- 

step estimation and prediction for each set of four transit 

time observables (T , T , T , and T ) . This one step process 

LX Y ^ 

exhibits the following salient features: 
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1. The four transit times received during each 1.31 
second measurement interval (time slot) are processed simul- 
taneously by the filter equations. Updated covariance of 
error and state estimates are therefore obtained only once 
for each time slot. 

2. A major part of the time period for the estimation 
calculation is spent in the gain equation where the inversion 
of a 4 x 4 matrix is required. 

3. The Extended Kalman filter is based on a linearization 
about the predicted value of a period t^ to t^+l. Unaccept- 
ably large or erroneous transit time measurements will cause 
the filter to ignore the update information for the entire 
time slot. The predicted estimate will become the new esti- 
mate for the time slot and the interval extended. 

If one assumes that each of the four hydrophones in the 
array are statistically independent of one another, a se- 
quential approach can be taken to process each of the four 
transit times separately. Thus, each observable will be 
processed in sequence, and the result of processing one 
measurement is used in the following computation to process 
the next measurement until all four observables have been 
used . 

The advantages of the sequential approach over the tra- 
ditional one-step estimation and prediction are: 

1. Since the four transit times are independent and 
processed sequentially, the covariance of error matrix and 
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the state vector are updated four times during each time 
slot. Thus more accurate estimates of the filter states 
are achieved. 

2. Modification of the filter equations for the sequen- 
tial approach circumvented the matrix inversion in the gain 
equation. Therefore filter cycle time is reduced substantially. 

3. An invalid transit time measurement will result in 
the filter ignoring the update information for that particular 
measurement only. For example, if only three of the four 
transit times are acceptable, the filter would be sequentially 
updated three times during that particular time slot. Thus, 
the filter is less sensitive to erroneous transit time 
measurements . 

To utilize the sequential Extended Kalman filter, equations 
(3.8) must be modified. Calculations are performed on each of 
the four independent transit times in the following order: T c , 

T , T , and T for each 1.31 second time slot. The estimate 
of the states x(k/k), based on one transit time measurement 
are used as the prediction x(k/k-l) for the calculations on 
the next measurement. Thus for the first time measurement T c 
only the first row of the linearizing H matrix is calculated. 

Next the first gain column corresponding to the first 
time measurement T^ is calculated by 



P (k/k-1) Ht 



T 



irow 



(4.19) 
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T 




irow 
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where i = 1 to 4 corresponding to the four measured transit 
times. Thus, the first row of the H matrix is used to calcu- 
late the first column of the gain matrix with both correspond- 
ing to the first measured time T c . 

Next, an estimate of the particular observation time is 
calculated using equation (3.8e) evaluated at the predicted 
state x(k/k-l) . 

The difference between observed transit time and the 
estimated transit times forms the residual which is used in 
the estimate equation 



x^ = x(k/k-l) + G ^ co ^ [Residual] (4.20) 

This equation gives an estimate of the states based on one 
measurement. 

Next, the covariance of error is calculated based on one 
measurement by 



P . 

l 



[I - G. ,H. ]P. . 

icol irow i-I 



(4.21) 



where 



I = identity matrix 

p i_l = the covariance matrix calculated from the pre- 
vious transit time measurement or if i = 1, 
the prediction P(k/k-l) . 

After the first iteration, x 1 becomes x(k/k-l) and P 1 becomes 
P(k/k-l) for the second iteration which calculates the estimate 
of the states based on the second measurement T x> 
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After four iterations (k = 4) , x^ becomes the estimate for 
the time slot x(k/k) and p^ becomes the covariance of error 
P (k/k) . 

The predictions for the next time slot are calculated 
using equations (3.8a) and (3.8d). This process is repeated 
for each time slot. 
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IV. TESTING AND SIMULATION 



A. DESCRIPTION 

The sequential Extended Kalman filter routine is tested 
using simulated torpedo tracks generated by the IBM-360 com- 
puter. A variety of track scenarios are produced to test the 
filter performance during single array tracking. 

The first series of tests used constant course and speed 
tracks which transitioned multiple array quadrants including: 

1. crossing north of the array 

2. crossing above the array 

3. crossing diagonally through the array 

The next series of tests adds a number of typical torpedo 
maneuvers to the constant course and speed tracks. These ma- 
neuvers consisted of 10 and 20 °/sec turns with G forces 
ranging from 1/4-G to 1-G. 

For the above tests filter initialization errors range 
from 0 to 40 feet/sec for X and Y velocity, and 0 to 60 feet 
for X and Y position. The Z depth is maintained constant at 
300 feet which is approximately the average depth of water 
at Dabob bay. Zero mean Gaussian noise is added to corrupt 
the observed transit times for all runs . 

The last series of tests demonstrates the ability of 
the filter to track through the areas of multiple arrays. 

Before the above series of tests can be conducted, it 
is necessary to add operational features to the software which 
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provide the following capability: 

1. detect erroneous transit time measurements 

2. minimize position error during high dynamic maneuvers 

3. track through multiple arrays 

A more detailed explanation of these features is described 
below . 

B. EDITING ERRONEOUS TIME MEASUREMENTS 

The operation of the filter may be adversely affected by 
erroneous transit time measurements. Large errors in the 
transit time measurements will produce errors in the state 
vector estimate and the linearizing H matrix. This may cause 
the filter to become unstable and exhibit divergent character- 
istics. In some extreme cases catastrophic failure may result. 
Measurement errors can occur because of many factors including 
an error in the transit time of the acoustic pulse primarily 
due to the receipt of multi path signals from previous time 
slots that have reflected off the surface, bottom, or different 
density layers in the water. 

To guard against catastrophic failures a three-sigma gate 
using the covariance of measurement noise (R) and the covari- 
ance of estimation error (P(k/k)) is implemented. For each 
calculation of a state estimate (x(k/k) ) , the largest posi- 
tional covariance of error is used, either x, y, or z_, and 
converted to time in seconds using the average velocity of 
sound in water for Dabob bay, 4860 ft/sec. The gate then is 
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written for each time measurement i 



1 to 4 : 



Gate 




maximum 




4860 



where j =1, 3, 5. The gate expands or decreases depending 
on the confidence level of the position estimate and the 
transit time. If ZDIFF which is the difference between the 
actual transit time received and the predicted transit time 
to a particular hydrophone exceeds the gate, the measurement 
is considered unacceptable and the filter gain is set to 
zero causing the filter to ignore the data and take the pre- 
diction of the states as the estimate 



An invalid time measurement zeros only the gain column for 
that particular hydrophone causing only that hydrophone's 
data to be ignored. 

C. BOUNDING RESIDUAL BIAS ERROR 

As previously mentioned, the torpedo dynamics are modeled 
in the filter's excitation covariance matrix Q. 

During preliminary tests it becomes evident that torpedo 
turn rates between 5 and 20°/sec yield position errors ranging 
from approximately 10 to 40 ft. Increasing the standard devi- 
ation parameters (a: o. a.) for the adaptive Q matrix helps 
to a certain extent, however as expected a point is reached 
where the noise starts to become the dominant error source. 
There are two major reasons why the filter develops errors 
during turns : 



x (k/k) = x (k/k-1) 
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1. The filter's predicted state estimates are computed 
assuming a constant velocity over a 1.31 second sampling 
interval . During a turn the velocity vector of the torpedo 
is continuously rotating, therefore the filter's predicted 
estimate will be in error. A large magnitude error will re- 
quire more than one time slot to remove. A faster sampling 
rate would reduce the error and may alleviate the problem; 
however, the Dabob tracking range system has been designed 
for a 1.31 second sampling rate and cannot be changed. 

2. The adaptive Q matrix assumes that the torpedo ac- 
celerations are normally distributed and uncorrelated. Torpedo 
tracking maneuvers do not fit these assumptions. 

To minimize position errors during maneuvers a procedure 
has been developed in order that repeated estimates of the 
state vector are obtained during a single sampling period 
(1.31 second time slot) until the transit time residual error 
falls below a preset threshold. 

In Section IV it was stated that during each time slot 
the gain, covariance, and state estimate equations (4.19— 

4.21) were iteratively solved for each of the four sequential 
transit times T , T v , T„, and T„ . Subsequently additional 
software was incorporated to calculate the estimated transit 
time to each of the four hydrophones based on the last up- 
dated state vector. Four time residuals are then computed 
by subtracting the estimated time to each hydrophone from the 
observed transit time. The average of the absolute value of 
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the time residuals is then formed. If the average residual 
error exceeds the preset threshold in the software, which is 
indicative of large error in the state vector, Q is calculated 
and added to the last updated covariance of error matrix P. 

The filter then reiterates the gain, covariance, and state 
estimate equations for the same time slot. If the new state 
vector generates an average residual error still exceeding the 
threshold value, then the process is repeated. This procedure 
continues until the average residual error falls below the 
preset threshold at which time an acceptable state vector 
estimate has been obtained for the time slot. Once this oc- 
curs the prediction equations (3.8a) and (3.8d) are solved 
and the filter proceeds to the next time slot and commences 
processing new transit time measurements. As will be shown 
later the filter did successfully bound the position error 
during turning maneuvers. For these tests the threshold con- 
stant is set at 10 microseconds, which is the value of RMS 
measurement noise. 

C. MULTIPLE ARRAY TRACKING 

Initial tests were performed on tracks in the area of 
one array. In order to more closely simulate a typical run 
on the range, a scheme was designed to track the torpedo 
through multiple arrays [2]. 

First, a coordinate system is defined as shown in Figure 
3. The center of the coordinate system is geographically 
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Coordinate System for Multiple Array Tracking 
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HYDRO — Hydrophone Location Matrix 



Figure 3 
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near the entrance to Dabob bay in the simulation. Array num- 
ber 6 is the closest array to the coordinate center. In the 
simulation array 1 is at 36,000 feet from coordinate center 
and array 6 is 6,000 feet. The C hydrophone is assumed to be 
the axis location of each array. Then each X position for the 
X hydrophone in each array is X c + 30, each Y position for the 
Y hydrophone is Y c + 30 and each Z position for the Z hydro- 
phone is Z c + 30. These 72 positions, an XYZ position for 
each of 4 hydrophones in 6 arrays, are placed into a 6 x 12 
matrix HYDRO and referenced throughout the routine. The 
torpedo position is referenced to a central level rectangular 
coordinate system. The nonlinear observation equations become 



T" 1 

n 

w 1 




VEL( (x ’ (k) - X iC ) ‘ + <>'’ (k) - Y lc ) ' + < 2 ' (k) - Z iC ) 2 l a 


V k) 




VELf (X ’ (k) - X iX ) 2 +< !’ ,<k; -' ( iX ) 2 +< 2 ' (k) - Z iX )2ia 


v*° 




^— [ (x* (k) - X ±y ) 2 + (y ' (k) -Y iY ) 2 + ( z ’ ( k) - Z ) 2 ] % 


_V k) _ 




7 i r [( ); '(k)-X . z ) 2 + (y’(k)-Y iZ ) 2 + ( Z '(k)-Z iZ ) 2 ] % 



where T c (k), T x (k) , T (k) , and T g (k) are the transit times to 
each of the four hydrophones and X' (k) , Y' (k) , and Z' (k) are 
the filter estimates x’(k/k) evaluated at the predicted 
positions x(k/k— 1) . The subscripted variables X, Y, and Z 
are the coordinates of a particular array being used. 
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The decision parameter used to determine the switching 
from array to array is a straight handoff. If the predicted 
x position is greater than 3,000 feet from the array in use, 
then an index (18) is incremented and the next row of HYDRO 
is implemented. This placed into the routine the X, Y, and Z 
positions of the hydrophones in the next array. The handoff 
can easily be utilized in real range operations, as the 
transit times from adjacent arrays are present at the compu- 
ter for a particular time slot. For simulation, it is as- 
sumed that in all the arrays each axis pointed in the same 
direction. In actual range operations each array is tilted 
about both the X and Y axis. Since the true transit times 
are derived in a tilted coordinate frame, the filter's esti- 
mate of transit time must also be calculated in a tilted 
coordinate frame. The tilt angle measurements along with 
the level rectangular coordinates of the array with respect 
to the central rectangular coordinate system can be input 
into the matrix HYDRO to rotate the coordinates of each 
hydrophone in the array. 
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VI. TEST RESULTS AND DISCUSSION 



A. SERIES ONE — STRAIGHT RUNS 

This series of tests includes constant speed and heading 
tracks. Initial position and velocity errors are set to 
zero. Measurement noise is added to all runs. Torpedo ve- 
locity ranged from 20 to 55 knots. Figure 4 shows the true 
trajectory of the torpedo in the horizontal X-Y plane. The 
center of the hydrophone array is located at positional co- 
ordinates (0,0). Figure 5 describes the filter's RMS estimate 
of position error as a function of sampling time. The RMS 
estimates are obtained by taking the square-root of the ap- 
propriate diagonal terms of the covariance matrix. It should 
be noted that the propagation of RMS estimates are a function 
of the position of the torpedo with respect to the hydrophone 
array. As shows in Figure 4, if Y = 0, the torpedo path is 
inbound toward the array along the "X" axis. Upon examining 
Figure 5, it is evident that the RMS estimate of X position 
error or the uncertainty in the filter's X position estimate 
is small and relatively constant throughout the entire tra- 
jectory. The uncertainty in the filter's Y position estimate 
decreases as the torpedo approaches the array, becomes a mini- 
mum at the closest point of approach (CPA) , and increases as 
the torpedo moves outbound away from the array. This behavior 
in the filter's RMS estimates can be explained by recognizing 
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that the cluster of four hydrophones in the array actually 
describe a position fixing system. The torpedo position is 
located by the intersection of circular lines of position 
(range arcs) originating from each hydrophone on the array. 

In general, the greater distance the torpedo is located from 
the array, the shallower the range arc intersections become 
and the more uncertainty exists in the actual torpedo posi- 
tion. For the case where the torpedo is moving along the X 
axis, the uncertainty in position exists only in the Y 
direction. If the torpedo's X distance from the array 
becomes infinite, the range arcs from each hydrophone appear 
to originate from a single point source. The position fixing 
system then reduces to a single dimension ranging system along 
the X axis with the uncertainty in the Y position estimate 
becoming infinite. If the torpedo approaches the array along 
the Y axis (X '= 0) , the filter's RMS estimate of Y position 
error will be relatively small, and as the torpedo's Y distance 
from the array becomes infinite, the filter's uncertainty in 
the X position estimate becomes infinite. 

In order to keep RMS position error estimates within 
allowable tolerance the Dabob range tracking system requires 
the torpedo remain within a 3,000 foot radius of the center of 
the hydrophone array. 

Figures 6 and 7 depict the error in both position and 
depth for the straight run trajectory along the X axis. 

The position errors are computed by subtracting the filter 
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position estimate, x(k/k) , from the computer generated true 
position for each time slot. From Figure 6, it is observed 
that the Y position error magnitude, which is caused by 
system noise, decreases on the inbound leg and increases on 
the outbound leg. Since the torpedo path originated on the 
outer tracking perimeter (X = 3,000 feet) , the noise in the 

Y channel will be amplified due to the large uncertainty in 
the Y position estimate. The magnitude of noise error in the 

Y direction also decreases as the torpedo approaches the ar- 
ray and increases as the torpedo moves outbound. The steady 
state X and Y position errors ranged between -11 and +4 feet 
throughout the trajectory. 

Figure 8 describes a straight inbound torpedo path that 
passes north of the array at a constant Y distance of 2121 
feet. Torpedo velocity is maintained at 20 knots. A compari- 
son of the RMS x estimates of this track with the preceding 
one. Figures 5 and 9, reveal that both estimates are relatively 
constant, however the X estimate in Figure 9 is significantly 
larger. Since the torpedo path is parallel to the X axis, a 
larger uncertainty in the X position estimate will result. 

A comparison of the RMS Y position error estimates reveals 
identical propagation characteristics. This is due to the 
symmetry in the east-west direction of the two trajectories. 

Figures 10 and 11 show that the steady state position 
and depth magnitude errors are less than 10 feet. The noise 
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induced error in the X position is due to the uncertainty in 
the filter X position estimate. 

A torpedo path which is inbound along a 45° diagonal to 
the array is depicted in Figure 12. For this run torpedo 
velocity is constant at 28 knots. Figure 13 shows that the 
filter's X and Y RMS estimates of position error are approxi- 
mately equal. Likewise Figure 14 shows that X and Y position 

error propagation is also similar. These results are consis- 
tent since the X and Y position magnitudes of the torpedo 
trajectory are identical. At the point of closest approach 

(X = 0, Y = 0) the uncertainty in both the X and Y positions 

becomes a minimum and increases as the torpedo moves past 
the array. Steady state position and depph errors range 
between -4 and +6 feet. 

B. SERIES TWO — MANEUVERING RUNS 

The results of the straight run analyses discussed in the 
previous subsection show that the propagation of the filter's 
estimate of RMS position error is dependent on the path of 
the torpedo with respect to the hydrophone array. Thus the 
erros in X and Y position, and Z depth are also a function of 
the torpedo trajectory. In this series of tests maneuvers 
are added to the three straight path trajectories. These 
maneuvers consist of 360° turns at turn rates of 10 and 20°/sec. 
G forces range between 1/4 -G and 1-G. G forces are changed 
using the same turn rate by varying velocity. A typical test 
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run on the tracking range at Dabob would include these type 
maneuvers . 

Figure 16 describes an inbound torpedo path which main- 
tains a Y distance from the array between -200 and +600 feet. 
Initial position and velocity are 30 and 20 feet in X and Y 
position and 30 and -20 feet/second in X and Y velocity. 

Torpedo velocity is maintained constant at 20 knots through- 
out the trajectory. Four 360° 1/4-G turns are executed during 
the run. Figures 17 and 18 describe the position and depth 
errors. Within approximately three time slots the filter 
achieved steady state performance. Upon observing the error 
propagation it is apparent that the filter is insensitive to 
turn rate maneuvers. Negligible degradation in position and 
depth is observed. Thus the error bound routine discussed in 
the previous section successfully bounds position and depth 
errors during turns. As expected the position errors decrease 
as the torpedo approaches the array and increase as the torpedo 
proceeds outbound from the array. Since position and depth 
errors exhibit approximately equal oscillations about zero 
indicates that the system noise is the dominant error source 
driving the filter. Bias errors on the time residuals due 
to maneuvers are negligible. Steady state errors in position 
and depth ranged between -1 and +5 feet throughout the run. 

The next test describes an inbound torpedo trajectory, 
Figure 19, which passes north of the hydrophone array. The 
torpedo maintained a Y distance from the array between 1600 
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and 2400 feet. Four 10°/sec 1/4-G turns are executed during 
this test. Filter initialization errors are 20 and 11 feet 
in X and Y position and 20 feet/sec in both X and Y velocity. 
The error in position and depth is described in Figures 20 
and 21. The X position error demonstrates larger deviations 
than the Y position error. As noted during the straight runs 
this is due to the difference in the filter's X and Y RMS 
position error estimates for this type trajectory. The oscil 
latory characteristics of the position error indicate again 
that noise is the dominant error source and the filter is 
tracking the torpedo through the maneuvers. Position errors 
range between -8 and +8 feet with one X channel error reach- 
ing a maximum of -15 feet, which occurred at time slot 140. 
The exact cause of the -15 foot error is unknown but may 
have been caused by an anomaly in the IBM- 360 subroutine 
SNORM which generates the zero mean Gaussian noise for the 
filter. Errors in torpedo depth, Figure 21, are consistent 
and range between -2 and +5 feet. 

For the next test the torpedo exhibits an inbound tra- 
jectory along a 45° diagonal with respect to the array. 

Figure 22 shows that four 10°/sec 1/4-G turns are executed 
on this run: 2 on the inbound leg and 2 on the outbound leg. 

Filter initialization errors are 60 feet in both X and Y 
position and 30 feet in X and Y velocity. Steady state con- 
ditions are achieved in approximately four time slots. As 
expected for this type trajectory, the X and Y position 
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errors portray similar propagation characteristics. Figure 23. 
This similarity can be explained by the equivalent X andY RMS 
error estimates generated by the filter for a 45° diagonally 
inbound trajectory. Position and depth errors. Figure 24, rnged 
ranged between -7 and +10 feet. 

The preceeding tests discussed filter performance for 
straight runs with 10°/sec 1/4-G turns. Using the same basic 
torpedo trajectories as above, similar tests are performed with 
10 °/sec turn rates at 1/2-G. Figures 25 through 33 describe 
the torpedo paths and the corresponding position and depth 
error propagation plots for each run. Filter initialization 
errors ranged between 10 and 40 feet in position and 10 and 
40 feet in velocity. Torpedo velocity is maintained constant 
at 55 knots. An analysis of the position and depth plots 
reveals that filter performance is similar to that described 
in the previous tests. In Figures 26 and 32 the larger errors 
incurred after time slot 135 are due to the torpedo trajectory 
exceeding the radial tracking range limit of 3,000 feet. This 
is more clearly described in the torpedo trajectory curves. 
Figures 25 and 31. The additional 1/4-G acceleration during 
the turns did not produce visible degradation in the filter's 
position and depth estimates. 

The next set of maneuvering tests increased the turn rate 
to 20°/sec with G forces at 1/2-G. The same basic inbound and 
outbound straight runs trajectories are used. Initialization 
errors ranged between 20 and 40 feet in position and -10 and 
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and 35 feet/second for velocity. Figure 34 describes a torpedo 
inbound trajectory which maintains a Y distance from the array 
between —275 and 500 feet. Four turns are executed on this 
run. The X and Y position errors reveal essentially the same 
characteristics described before. In general the position and 
depth errors decreased as the torpedo approaches the array, 
become a minimum at the closest point of approach, and increase 
as the torpedo moves outbound past the array. The higher turn 
rates experienced on this test do not degrade the X and Y posi- 
tion errors. Figures 35 and 36 show that position and depth 
errors ranged between -1 and +5 feet. 

Figure 37 depicts a torpedo path which maintains a Y dis- 
tance from the array between 1760 and 2760 feet. Three 20°/sec 
1/2-G turns are performed during this run. Figures 38 and 39 
describe the position and depth errors which range between -7 
and +9 feet during steady state. 

The diagonally inbound trajectory with four 20°/sec 1/2-G 
turns is displayed in Figure 40. Position and depeth errors 
for this run. Figures 41 and 42, ranged between +10 and -10 
feet. Again note the similarity in the position error behavior 
for each channel for this type trajectory. The larger errors 
in position after time slot 125 are due to the filter's RMS 
position error estimates increasing as the torpedo approaches 
the radial tracking limit of the hydrophone array. 

The last set of maneuvering tests increased the G force 
during the 20°/sec turns from 1/2-G to 1-G. The three basic 
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inbound-outbound trajectories are again repeated. Figures 
43 through 51 describe the torpedo trajectories and correspond- 
ing position and depth errors. Overall performance indicates 
that during steady state conditions the magnitude of position 
and depth errors are below 10 feet. 

Due to the presence of system noise position errors 
reached a maximum of 14 feet, when the torpedo trajectory 
exceeded the radial tracking limit of the hydrophone array. 

As was also observed in the preceding tests, errors induced 
by the turn rate appeared to have a negligible effect on fil- 
ter performance. 

C. SERIES THREE — MULTIPLE ARRAY TRACKING 

The purpose of the last series of tests is to functionally 
demonstrate that the filter is able to track the torpedo 
through multiple arrays using the simulated tracking range 
set up and handoff scheme described in the previous section. 
Figure 52 depicts a straight run trajectory which traverses 
the six hydrophone arrays. The position and depth errors 
are described in Figures 53 and 54. 

Note that the handoff from array to array was accomplished 
with negligible effect on position and depth errors. The mag- 
nitude of the errors compare well with those observed on the 
single array straight run tracking. 



44 



VII. CONCLUSIONS 



The sequential Extended Kalman filter satisfactorily pro- 
vided real time estimates of torpedo position and depth. The 
magnitude of steady state position and depth errors ranged 
between 0 and 10 feet for torpedo tracks within the specified 
radial tracking range. 

The iterative error bound routine furnished the capability 
to estimate torpedo position and depth during turning maneu- 
vers up to 1-G. Errors generated during maneuvers are compa- 
rable to those observed during straight runs . 

The filter's RMS estimates of position and depth errors 
are dependent on system noise and the distance the torpedo 
is from the center of the hydrophone array. In general larger 
errors are encountered at more remote distances from the 
hydrophone array. 

The simulated trajectories were performed using constant 
measurement noise sequences. Since the filtering problem is 
non-linear the error, in the filter's state estimate vector is 
a function of the statistical properties of the noise. Future 
tests should include a Monte Carlo simulation where filter 
performance is investigated for an ensemble of measurement 
noise sequences. 

Additional tests should also include evaluating filter 
performance using trajectories generated from actual torpedo 
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runs on the Dabob test range. These tests would verify the 
adequacy of the noise model in the filter and the ability of 
the software to edit erroneous transit time measurements. 
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Figure 5. Filter Estimate of RMS Position Error During a 
Straight Run in the Area of a Single Array 
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Figure 6. Error in Torpedo Position During A Straight Run 
in the Area of a Single Array 
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Figure 7. Error in Torpedo Depth During a Straight Run 
in the Area of a Single Array 
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Figure 10. Error in Torpedo Position During a Straight 
Run in the Area of a Single Array- 
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Figure 11. Error in Torpedo Depth During a Straight Run 
in the Area of a Single Array- 
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Figure 12. True Trajectory of the Torpedo During a 

Straight Run in the Area of a Single Array 
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Figure 13. Filter Estimate of RMS Position Position Error 
During a Single Run in the Area of Single Array 
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Figure 15. Error in Torpedo Depth During a Straight Run 
in the Area of a Single Array 
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Figure 16. True Trajectory of the Torpedo in the Area of 
a Single Array 
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Figure 17 . Error in Torpedo Position During a Maneuvering 
Run in the Area of a Single Array 
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Figure 18. Error in Torpedo Depth During a Maneuvering 
Run in the Area of a Single Array 



61 



-POSITION (FT) 



2600. 




1400. *— *- 

- 2000 . 



i i i i i i 
- 1000 . 0 . 



' ' 1 I ' ' 1 I ' ' ' I 

1000. 2000. 30C0 . 



X-POS I T I ON (FT) 



Figure 19. True Trajectory of the Torpedo During a Maneuvering 
Run in the Area of a Single Array 
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Figure 20. Error in Torpedo Position During a Maneuvering 
Run in the Area of a Single Array 
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ERROR IN Z-DEPTH (FT) 
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Note: Turn rate is 10 9 /sec at 28 knots. 
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Figure 21. Error in Torpedo Depth During a Maneuvering 
Run in the Area of a Single Array 
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Figure 22. True Trajectory of the Torpedo During a 
Maneuvering Run in the Area of A Single 
Array 
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Figure 23. Error in Torpedo Position During a Maneuvering 
Run in the Area of a Single Array 
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Figure 24. Error in Torpedo Depth During a Maneuvering 
Run in the Area of a Single Array- 
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Note 1: Hydrophone array coordinates are located 

at (0,0). 

Note 2: Trajectory exceeds radial tracking limit. 
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Figure 25. True Trajectory of the Torpedo During a 
Maneuvering Run in the Area of a Single 
Array 
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Figure 26. Error in Torpedo Position During a Maneuvering 
Run in the Area of a Single Array 
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Figure 27. Error in Torpedo Depth During a Maneuvering 
Run in the Area of a Single Array 
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Figure 28. True Trajectory of the Torpedo During a 

Maneuvering Run in the Area of a Single Array 
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Figure 29. Error in Torpedo Position Duirng a Maneuvering 
Run in the Area of a Single Array 
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Figure 30. Error in Torpedo Depth During a Maneuvering 
Run in the Area of a Single Array 
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Figure 31. True Trajectory of the Torpedo During a 
Maneuvering Run in the Area of a Single 
Array 
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Figure 32. Error in Torpedo Position During a Maneuvering 
Run in the Area of a Single Array 
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Figure 33. Error in Torpedo Position During a Maneuvering 
Run in the Area of a Single Array 
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Figure 34. True Trajectory of the Torpedo During a 

Maneuvering Run in the Area of a Single Array 
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Figure 35. Error in Torpedo Position During a Maneuvering 
Run in the Area of a Single Array 
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ERROR IN Z-DEPTH (FT) 
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Figure 36. Error in Torpedo Depth During a Maneuvering 
Run in the Area of a Single Array 
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Figure 37. True Trajectory of the Torpedo During a 
Maneuvering Run in the Area of a Single 
Array 
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Figure 38. Error in Torpedo Position During a Maneuvering 
Run in the Area of a Single Array 
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ERROR IN Z-QEPTH (FT) 
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Figure 39. Error in Torpedo Depth During a Maneuvering 
Run in the Area of a Single Array 
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Figure 40. True Trajectory of the Torpedo During a 
Maneuvering Run in the Area of a Single 
Array 
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Figure 41. Error in Torpedo Position During a Maneuvering 
Run in the Area of a Single Array 
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Figure 42. Error in Torpedo Depth During a Maneuvering 
Run in the Area of a Single Array 
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Figure 43. True Trajectory of the Torpedo During a 
Maneuvering Run in the Area of a Single 
Array 
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Figure 44. Error in Torpedo Position During a Maneuvering 
Run in the Area of a Single Array 
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Figure 45. Error in Torpedo Depth During a Maneuvering 
Run in the Area of a Single Array 
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Figure 46. True Trajectory of the Torpedo During a 

Maneuvering Run in the Area of A Single Array 
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Figure 47. Error in Torpedo Depth During a Maneuvering 
Run in the Area of a Single Array 
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Figure 48. Error in Torpedo Position During a Maneuvering 
Run in the Area of a Single Array 
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Figure 49. True Trajectory of the Torpedo During a 
Maneuvering Run in the Area of a Single 
Array 
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Figure 50. Error in Torpedo Position During a Maneuvering 
Run in the Area of a Single Array 
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Figure 51. Error in Torpedo Depth During a Maneuvering 
Run in the Area of a Single Array 
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Figure 52, True Trajectory of the Torpedo During a Straight Run Through Multiple 
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Figure 53. Error in Torpedo Position During a Straight Run Through 
Multiple Arrays 
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APPENDIX A 



PROGRAM DESCRIPTION AND FEATURES 

The sequential Extended Kalman Filter routine used for 
torpedo tracking is modularized for ease of implementation. 

The program is general in nature and many of the parameters 
of the filter are variable including: 

a. The number of states in the filter — N 

b. The number of random forcing functions — M 

c. The number of measurements — JS 

d. Number of time slots — JTIME 

The constant matrices PHI, R, COW, and GAMMA are initialized 
in the beginning of the program using data statements. The 
filter is initialized with P(l/0) and X(l/0) (initial covari- 
ance of estimation error and states) using subroutine INIT. 

The first estimate is at time 1 and continues until ITIME = 
JTIME+1 . True measurement times (ZI) are computed using either 
subroutines TRAJEC or TRAJC3 , depending on whether single 
array or multiple array tracking is implemented. Either sub- 
routine will compute four measurement times (T c , T^, T , T z ) 
for each time slot. The measurement times are corrupted by 
zero-mean, white Gaussian noise using the IBM-360 subroutine 
SNORM. For each of the four time measurements the corresponding 
row of the linearizing H matrix is calculated using either 
subroutine CHROW or CHR0W3, depending on whether single array 
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or multiple array tracking is used. The corresponding gain 
matrix column GI is then found. These row and column values 
are utilized in forming the covariance of estimation error, 

PI, for that particular time measurement. Next the estimate 
of the observation time ZHAT from that particular hydrophone 
is formed using the subroutine CZHAT or CZHAT3, depending on 
whether single or multiple array tracking is implemented. The 
residual ZDIFF(I) = ZIC(I) - ZHAT, is then calcualated. Finally 
the estimate of the states XI based on one time measurement is 
calculated and the process- is repeated for the next measure- 
ment. After the four iterations, four transit time measures 
are formed using the last estimate of the states, X4, in either 
subroutine CZHAT or CZHAT3. The corresponding four transit 
time residuals are then calculated. The average of the abso- 
lute value of the time residuals is formed (ZDIFAV) and com- 
pared to a preset threshold value in the software. If ZDIFAV 
is above the threshold, subroutine QFIND is used to calcuate 
a new Q which is then added to the last covariance of error 
estimate, P4 . The software then reprocesses the transit time 
measurements for the same time slot. This procedure continues 
until the ZDIFAV measurement is equal or below the threshold 
value, at which time the last estimate of the state vector 
and covariance matrix become the XKK and PKK values for the 
time slot. Finally, the predictions XKKMI and PKKMI are formed 
before the process is repeated for the next time slot. PTLOP 
is used to generate line printer plots and PLOTG is used to 
generate VERSATEC plots . 
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A. PROGRAM SUBROUTINES 



A brief description of the subroutines that are used 
with the filter are described below: 

1. TRAJEC — This subroutine develops the torpedo tra- 
jectory which is used as truth data for the filter. The 
subroutine generates constant velocity and heading tracks, 
turns of any desired turn rate and G force can be performed 
at any time along the torpedo track. The subroutine outputs 
true transit times, ZI (I) , and the X, Y, Z positions, TD(I), 
of the torpedo for each time slot. The subroutine is used 
during single array tracking. 

2. TRAJC3 — This subroutine performs the same function 
as TRAJEC but is used only during multiple array tracking. 

3. INIT — This subroutine generates the initial state 
vector (X0/-1) and initial covariance matrix (P 0/-1) . 

4. CHROW — This subroutine computes the appropriate row 

of the linearizing H matrix. Each row corresponds to one 

of the four transit time measurements, T , T , T , T This 

C A Y u 

subroutine is used during single state array tracking. 

5. CHR0W3 — THis subroutine performs the same functions 
as CHROW and used during multiple array tracking. 

6. CZHAT — This subroutine computes the estimated transit 
times for the filter. Four time estimates, ZHAT , are calcu- 
lated corresponding to each of the four true transit times 
ZI(I). This subroutine is used during single array tracking. 

7. CZHAT3 — Subroutine performs same functions as CZHAT 
however it is used only during multiple array tracking. 
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8. QFIND — This subroutine develops an adative Q matrix 
which is a function of the torpedo velocity. Three input 
variables defined in a data statement at the beginning of the 
program can be adjusted: 

aa. SIGACC — Maximum expected horizontal acceleration 
of the torpedo. 

bb. SIGDIV — Maximum expected change in vertical 
velocity . 

cc. SIGCC — Maximum expected turn rate of the torpedo 
in the horizontal plane. 

The values listed in the program were used and kept 
constant during the simulation tests. If the user desires 
not to use the adaptive Q subroutine software code is provided 
at the beginning of the program to calculate a constant Q 
matrix. 

9. SNORM — This is an IBM-360 subroutine contained in the 
IMSL library. The routine generates zero mean white Gaussian 
noise with an RMS value normalized to 1. The main program 
scales the noise and adds it to the transit times measurements. 

10. PLOT? — This is an IBM-360 subroutine used to generate 
the line printer plots. Information on this subroutine can 
be obtained from the IMSL library. 

11. PLOTG — This is an IBM-360 subroutine used to generate 
the VERSATEC plots. Information on this subroutine can be 
obtained from the IMSL library. 



101 



B. UTILITY PROGRAMS 



These subroutines were designed to be used for repetitive 
matrix and vector manipulations: 

1. PROD — multiplying two matrices 

2. MMULT — multiplying a matrix and a vector 

3. VMULT — multiplying two vectors 

4. TRANS — transposing a matrix 

5. ADD — adding two matrices 
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APPENDIX B 

SEQUENTIAL EXTENDED KALMAN FILTER PROGRAM LISTING 
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